#ifndef _AMERICO_OCT_TREE_CELL_H_
#define _AMERICO_OCT_TREE_CELL_H_

#include <growingarray.h>
#include <staticarray.h>
#include <Intersection/Box.h>

//#define OCT_TREE_MAX_OBJECTS 3
//#define OCT_TREE_MAX_DEPTH 4

enum OctTreeChilds
{
	UP_NORTH_WEST,
	UP_NORTH_EAST,
	UP_SOUTH_WEST,
	UP_SOUTH_EAST,
	DOWN_NORTH_WEST,
	DOWN_NORTH_EAST,
	DOWN_SOUTH_WEST,
	DOWN_SOUTH_EAST,
	OCT_TREE_CHILDS
};

class Instance3D;

#pragma warning (push)
#pragma warning(disable: 4127)

template <typename T>
class OctTreeCell
{
public:
	OctTreeCell();
	~OctTreeCell();

	void Init(const Vector3<float> &aCenter, const Vector3<float> &someExtents, const unsigned int &aNumberOfObjects=3, const unsigned int &aMaxDepth=4);
	void Init(const Box &aBox, const unsigned int &aNumberOfObjects=3, const unsigned int &aMaxDepth=4);

	inline void SetDynamic(const bool &aDynamicFlag=true) { myAmDynamic=aDynamicFlag; }
	inline void SetMaxObjects(const unsigned int &aNumberOfObjects) { myMaxObjects=aNumberOfObjects; }
	inline void SetMaxDepth(const unsigned int &aMaxDepth) { myMaxDepth=aMaxDepth; }
	inline void SetRoot(OctTreeCell *aOctTreeCell) { myRoot = aOctTreeCell; }

	void AddObject(T *aDweller);
	bool ObjectFits(const unsigned int &anIndex, T *anObject);
	bool ObjectFits(T *anObject);

	bool RemoveObject(T *aDweller);

	inline const Box &GetBox() const { return myBox; }

	inline OctTreeCell *GetChild(const unsigned int &anIndex) const { return myChilds[anIndex]; }
	inline unsigned int NumberOfChilds() const { return myChilds.size(); }

	inline T *GetDweller(const unsigned int &anIndex) const { return myDwellers[anIndex]; }
	inline unsigned int size() const { return myDwellers.Count(); }

	inline OctTreeCell *GetRoot() const { return myRoot; }
	inline unsigned int Depth() const { return myDepth; }

	inline bool IsDynamic() const { return myAmDynamic; }
	inline unsigned int MaxObjects() const { return myMaxObjects; }
	inline unsigned int MaxDepth() const { return myMaxDepth; }

private:
	Box myBox;
	unsigned short myDepth;

	bool myHaveChildren;
	Utils::StaticArray<OctTreeCell*, OCT_TREE_CHILDS> myChilds;
	Utils::StaticArray<Box, OCT_TREE_CHILDS> myChildBoxes;
	Utils::GrowingArray<T*> myDwellers;

	OctTreeCell *myRoot;

	bool myAmDynamic;
	unsigned int myMaxObjects;
	unsigned int myMaxDepth;

	void CreateChild(const unsigned int &anIndex);
};

template <typename T>
OctTreeCell<T>::OctTreeCell()
{
	myDwellers.Init(100, 50);
	myHaveChildren = false;
	myBox.Init(Vector3f(0.0f, 0.0f, 0.0f), Vector3f(0.0f, 0.0f, 0.0f));
	for (unsigned short index=0; index<myChilds.size(); index++)
	{
		myChilds[index] = 0;
		myChildBoxes[index].Init(Vector3f(0.0f, 0.0f, 0.0f), Vector3f(0.0f, 0.0f, 0.0f));
	}
	myDepth = 0;
	myRoot = 0;
	myAmDynamic = true;
	myMaxDepth = 4;
	myMaxObjects = 3;
}

template <typename T>
OctTreeCell<T>::~OctTreeCell()
{
	for(unsigned short index=0; index<myChilds.size(); index++)
	{
		delete myChilds[index];
		myChilds[index] = 0;
	}
}

template <typename T>
void OctTreeCell<T>::Init(const Vector3<float> &aCenter, const Vector3<float> &someExtents, const unsigned int &aNumberOfObjects, const unsigned int &aMaxDepth)
{
	myMaxObjects = aNumberOfObjects;
	myMaxDepth = aMaxDepth;
	myBox.Init(aCenter, someExtents);
	Vector3f extents = myBox.myExtents * 0.5f;
	myChildBoxes[UP_NORTH_WEST].Init(myBox.myCenterPos+Vector3f(-extents.x, -extents.y, -extents.z), extents);
	myChildBoxes[UP_NORTH_EAST].Init(myBox.myCenterPos+Vector3f( extents.x, -extents.y, -extents.z), extents);
	myChildBoxes[UP_SOUTH_WEST].Init(myBox.myCenterPos+Vector3f(-extents.x, -extents.y,  extents.z), extents);
	myChildBoxes[UP_SOUTH_EAST].Init(myBox.myCenterPos+Vector3f( extents.x, -extents.y,  extents.z), extents);
	myChildBoxes[DOWN_NORTH_WEST].Init(myBox.myCenterPos+Vector3f(-extents.x,  extents.y, -extents.z), extents);
	myChildBoxes[DOWN_NORTH_EAST].Init(myBox.myCenterPos+Vector3f( extents.x,  extents.y, -extents.z), extents);
	myChildBoxes[DOWN_SOUTH_WEST].Init(myBox.myCenterPos+Vector3f(-extents.x,  extents.y,  extents.z), extents);
	myChildBoxes[DOWN_SOUTH_EAST].Init(myBox.myCenterPos+Vector3f( extents.x,  extents.y,  extents.z), extents);
	if ((myAmDynamic==false) && (myDepth<myMaxDepth))
	{
		myHaveChildren = true;
		for (unsigned short index=0; index<myChilds.size(); ++index)
		{
			if (myChilds[index]==0)
			{
				CreateChild(index);
			}
			myChilds[index]->myDepth = myDepth + 1;
			myChilds[index]->myRoot = myRoot;
		}
	}
}

template <typename T>
void OctTreeCell<T>::Init(const Box &aBox, const unsigned int &aNumberOfObjects, const unsigned int &aMaxDepth)
{
	myMaxObjects = aNumberOfObjects;
	myMaxDepth = aMaxDepth;
	myBox = aBox;
	Vector3f extents = myBox.myExtents * 0.5f;
	myChildBoxes[UP_NORTH_WEST].Init(myBox.myCenterPos+Vector3f(-extents.x, -extents.y, -extents.z), extents);
	myChildBoxes[UP_NORTH_EAST].Init(myBox.myCenterPos+Vector3f( extents.x, -extents.y, -extents.z), extents);
	myChildBoxes[UP_SOUTH_WEST].Init(myBox.myCenterPos+Vector3f(-extents.x, -extents.y,  extents.z), extents);
	myChildBoxes[UP_SOUTH_EAST].Init(myBox.myCenterPos+Vector3f( extents.x, -extents.y,  extents.z), extents);
	myChildBoxes[DOWN_NORTH_WEST].Init(myBox.myCenterPos+Vector3f(-extents.x,  extents.y, -extents.z), extents);
	myChildBoxes[DOWN_NORTH_EAST].Init(myBox.myCenterPos+Vector3f( extents.x,  extents.y, -extents.z), extents);
	myChildBoxes[DOWN_SOUTH_WEST].Init(myBox.myCenterPos+Vector3f(-extents.x,  extents.y,  extents.z), extents);
	myChildBoxes[DOWN_SOUTH_EAST].Init(myBox.myCenterPos+Vector3f( extents.x,  extents.y,  extents.z), extents);
	if ((myAmDynamic == false) && (myDepth<myMaxDepth))
	{
		myHaveChildren = true;
		for (unsigned short index=0; index<myChilds.size(); ++index)
		{
			if (myChilds[index]==0)
			{
				CreateChild(index);
			}
			myChilds[index]->Init(myChildBoxes[index], myMaxObjects, myMaxDepth);
			myChilds[index]->myDepth = myDepth + 1;
			myChilds[index]->SetDynamic(myAmDynamic);
		}
	}
}

template <typename T>
void OctTreeCell<T>::AddObject(T *aDweller)
{
	if (myHaveChildren)
	{
		for(unsigned int index=0; index<myChilds.size(); ++index)
		{
			if(ObjectFits(index, aDweller))
			{
				if(myChilds[index]==0)
				{
					CreateChild(index);
				}
				myChilds[index]->AddObject(aDweller);
				return;
			}
		}
	}
	//unsigned int dwellerId = myDwellers.Count();
	myDwellers.Add(aDweller);
	aDweller->SetCell(this); //TODO
	unsigned int size = myDwellers.Count();
	if ((myDepth<myMaxDepth) && (size > myMaxObjects))
	{
		for (unsigned int objectIndex=0; objectIndex<myDwellers.Count(); ++objectIndex)
		{
			T *object = myDwellers[objectIndex];
			for(unsigned int index=0; index<myChilds.size(); ++index)
			{
				if(ObjectFits(index, object))
				{
					if(myChilds[index]==0)
					{
						CreateChild(index);
					}
					myChilds[index]->AddObject(object);
					myDwellers.RemoveCyclicAtIndex(objectIndex);
					--objectIndex;
					myHaveChildren = true;
					break;
				}
			}
		}
	}
}

template <typename T>
bool OctTreeCell<T>::ObjectFits(const unsigned int &anIndex, T *anObject)
{
	if (myDepth>=myMaxDepth-1)
	{
		return false;
	}
	if (myChilds[anIndex] && (myChilds[anIndex]->myDwellers.Count()>myMaxObjects))
	{
		return false;
	}
	
	//bool pointInside = Intersection::PointInsideAABB(myChildBoxes[anIndex], anObject->GetSphere().myCenterPosition);

	//Vector3f closest = ClosestPointAABB(anObject->GetSphere().myCenterPosition, myChildBoxes[anIndex]);
	//float dist = (anObject->GetSphere().myCenterPosition-closest).Length2();
	//bool sphereInside = (dist<=anObject->GetSphere().myRadiusSquared);

	float deltaX = anObject->GetSphere().myCenterPosition.x - myChildBoxes[anIndex].myCenterPos.x;
	if(abs(deltaX) + anObject->GetSphere().myRadius > myChildBoxes[anIndex].myExtents.x)
	{
		return false;
	}

	float deltaY = anObject->GetSphere().myCenterPosition.y - myChildBoxes[anIndex].myCenterPos.y;
	if(abs(deltaY) + anObject->GetSphere().myRadius > myChildBoxes[anIndex].myExtents.y)
	{
		return false;
	}

	float deltaZ = anObject->GetSphere().myCenterPosition.z - myChildBoxes[anIndex].myCenterPos.z;
	if(abs(deltaZ) + anObject->GetSphere().myRadius > myChildBoxes[anIndex].myExtents.z)
	{
		return false;
	}

	return true;
}

template <typename T>
bool OctTreeCell<T>::ObjectFits(T *anObject)
{
	if (myDwellers.Count()>myMaxObjects)
	{
		for (unsigned int index=0; index<myDwellers.Count(); index++)
		{
			if (myDwellers[index] == anObject)
			{
				bool pointInside = Intersection::PointInsideAABB(myBox, anObject->GetSphere().myCenterPosition);
				bool sphereInside = Intersection::SphereVsAABB(anObject->GetSphere(), myBox);
				if (pointInside && sphereInside)
				{
					return true;
				}
				return false;
			}
		}
		return false;
	}
	bool pointInside = Intersection::PointInsideAABB(myBox, anObject->GetSphere().myCenterPosition);
	bool sphereInside = Intersection::SphereVsAABB(anObject->GetSphere(), myBox);
	if (pointInside && sphereInside)
	{
		return true;
	}
	return false;
}

template <typename T>
bool OctTreeCell<T>::RemoveObject(T *aDweller)
{
	if (myDepth>=myMaxDepth)
	{
		return false;
	}
	for (unsigned int index=0; index<myDwellers.Count(); index++)
	{
		if (myDwellers[index] == aDweller)
		{
			myDwellers.RemoveCyclicAtIndex(index);
			return true;
		}
	}
	for (unsigned int index=0; index<myChilds.size(); index++)
	{
		if (myChilds[index] && myChilds[index]->RemoveObject(aDweller))
		{
			return true;
		}
	}
	return false;
}

template <typename T>
void OctTreeCell<T>::CreateChild(const unsigned int &anIndex)
{
	if (anIndex>=myChilds.size())
	{
		return;
	}
	myChilds[anIndex] = new OctTreeCell();
	myChilds[anIndex]->myDepth = myDepth + 1;
	myChilds[anIndex]->SetRoot(myRoot);
	myChilds[anIndex]->SetDynamic(myAmDynamic);
	myChilds[anIndex]->Init(myChildBoxes[anIndex], myMaxObjects, myMaxDepth);
}
#pragma warning (pop)

#endif